import autonomous.Boid;
import processing.core.PApplet;
import processing.core.PVector;

public class Wander extends PApplet {

	public static void main(String args[]) {
		PApplet.main(new String[] { "--present", "Wander" });
	}

	Boid wanderer;
	boolean debug = true;

	public void setup() {
		size(1200, 800);
		wanderer = new Boid(this, new PVector(width / 2, height / 4),
				(float) 5.0, (float) 5.1);
		smooth();
	}

	public void draw() {
		background(80, 25, 155);
		wanderer.wander();
		wanderer.run();
	}

	public void mousePressed() {
		debug = !debug;
	}

	class Boid extends PApplet {

		PVector loc;
		PVector vel;
		PVector acc;
		float r;
		float wandertheta;
		float maxforce; // Maximum steering force
		float maxspeed; // Maximum speed

		PApplet parent; //

		public Boid(PApplet p, PVector l, float ms, float mf) {
			parent = p; //
			acc = new PVector(0, 0);
			vel = new PVector(0, 0);
			loc = l.get();
			r = (float) 5.0;
			wandertheta = (float) 0.0;
			maxspeed = ms;
			maxforce = mf;
		}

		public void run() {
			update();
			borders();
			render();
		}

		// Method to update location
		void update() {
			// Update velocity
			vel.add(acc);
			// Limit speed
			vel.limit(maxspeed);
			loc.add(vel);
			// Reset accelertion to 0 each cycle
			acc.mult(0);
		}

		void seek(PVector target) {
			acc.add(steer(target, false));
		}

		void arrive(PVector target) {
			acc.add(steer(target, true));
		}

		public void wander() {
			float wanderR = 16.0f; // Radius for our "wander circle"
			float wanderD = 60.0f; // Distance for our "wander circle"
			float change = 0.25f;
			wandertheta += random(-change, change); // Randomly change wander
													// theta

			// Now we have to calculate the new location to steer towards on the
			// wander circle
			PVector circleloc = vel.get(); // Start with velocity
			circleloc.normalize(); // Normalize to get heading
			circleloc.mult(wanderD); // Multiply by distance
			circleloc.add(loc); // Make it relative to boid's location

			PVector circleOffSet = new PVector(wanderR * cos(wandertheta),
					wanderR * sin(wandertheta));
			PVector target = PVector.add(circleloc, circleOffSet);
			acc.add(steer(target, false)); // Steer towards it

			// Render wandering circle, etc.
			if (debug)
				drawWanderStuff(loc, circleloc, target, wanderR);

		}

		// A method that calculates a steering vector towards a target
		// Takes a second argument, if true, it slows down as it approaches the
		// target
		PVector steer(PVector target, boolean slowdown) {
			PVector steer; // The steering vector
			PVector desired = PVector.sub(target, loc); // A vector pointing
														// from
														// the location to the
														// target
			float d = desired.mag(); // Distance from the target is the
										// magnitude of
										// the vector
			// If the distance is greater than 0, calc steering (otherwise
			// return
			// zero vector)
			if (d > 0) {
				// Normalize desired
				desired.normalize();
				// Two options for desired vector magnitude (1 -- based on
				// distance,
				// 2 -- maxspeed)
				if ((slowdown) && (d < 100.0f))
					desired.mult(maxspeed * (d / 100.0f)); // This damping is
															// somewhat
															// arbitrary
				else
					desired.mult(maxspeed);
				// Steering = Desired minus Velocity
				steer = PVector.sub(desired, vel);
				steer.limit(maxforce); // Limit to maximum steering force
			} else {
				steer = new PVector(0, 0);
			}
			return steer;
		}

		void render() {
			// Draw a triangle rotated in the direction of velocity
			float theta = vel.heading2D() + radians(90);
			parent.fill(175);
			parent.stroke(0);
			parent.pushMatrix();
			parent.translate(loc.x, loc.y);
			parent.rotate(theta);
			parent.beginShape(TRIANGLES);
			parent.vertex(10, -r * 12);
			parent.vertex(-r, r * 12);
			parent.vertex(r, r * 12);
			parent.endShape();
			parent.popMatrix();
		}

		// Wraparound
		void borders() {
			if (loc.x < -r)
				loc.x = parent.width + r;
			if (loc.y < -r)
				loc.y = parent.height + r;
			if (loc.x > parent.width + r)
				loc.x = -r;
			if (loc.y > parent.height + r)
				loc.y = -r;
		}

		// A method just to draw the circle associated with wandering
		void drawWanderStuff(PVector loc, PVector circle, PVector target,
				float rad) {
			parent.stroke(0);
			parent.noFill();
			parent.ellipseMode(CENTER);
			parent.ellipse(circle.x, circle.y, rad * 2, rad * 2);
			parent.ellipse(target.x, target.y, 4, 4);
			parent.line(loc.x, loc.y, circle.x, circle.y);
			parent.line(circle.x, circle.y, target.x, target.y);
		}
	}
}
